#include <opencv2/opencv.hpp>
#include <iostream>
#include <string>


void generateRotation(const float Rx, const float Ry, const float Rz, cv::Mat &Rotx, cv::Mat &Roty, cv::Mat &Rotz)
{
	Rotx = cv::Mat::eye(3, 3, CV_32FC1);
	Roty = cv::Mat::eye(3, 3, CV_32FC1);
	Rotz = cv::Mat::eye(3, 3, CV_32FC1);

	Rotx.at<float>(1, 1) = cos(Rx);
	Rotx.at<float>(1, 2) = -sin(Rx);
	Rotx.at<float>(2, 1) = sin(Rx);
	Rotx.at<float>(2, 2) = cos(Rx);

	Roty.at<float>(0, 0) = cos(Ry);
	Roty.at<float>(0, 2) = sin(Ry);
	Roty.at<float>(2, 0) = -sin(Ry);
	Roty.at<float>(2, 2) = cos(Ry);

	Rotz.at<float>(0, 0) = cos(Rz);
	Rotz.at<float>(0, 1) = -sin(Rz);
	Rotz.at<float>(1, 0) = sin(Rz);
	Rotz.at<float>(1, 1) = cos(Rz);
}

void pano2Planet(cv::Mat panoImg, cv::Mat &planeImg, float fov, float distance, cv::Mat Rot, cv::Size colorSize)
{
	int panoH = panoImg.rows;
	int panoW = panoImg.cols;
	int channel = panoImg.channels();
	if (planeImg.empty())
	{
		planeImg = cv::Mat::zeros(colorSize, panoImg.type());
	}

	//project center
	float a = 0;
	float b = 0;
	float c = 1;
	cv::Point3f center(a, b, c);
	distance = sqrt(a*a + b*b + c*c);
	float harfL = (distance + 1)*tan(fov*0.5);
	float L = 2 * harfL;

	//sphere radius;
	uchar *panoData = (uchar*)panoImg.data;
	uchar *planeData = (uchar*)planeImg.data;
	float *pRot = (float*)Rot.data;
	for (int i = 0; i < colorSize.height; i++)
	{
		for (int j = 0; j < colorSize.width; j++)
		{
			float u = j*1.0 / colorSize.width;
			float v = i*1.0 / colorSize.height;

			cv::Point3f uv3D;
			uv3D.x = L * u - harfL;
			uv3D.y = harfL - (L * v);
			uv3D.z = (center.z > 0) ? (-1) : (1);

			float r = sqrt(uv3D.x*uv3D.x + uv3D.y*uv3D.y);
			if (r < 0.0001) continue;
			float fi = acos(uv3D.x / r);
			if (uv3D.y < 0)
			{
				fi = 2 * M_PI - fi;
			}

			float alpha = atan2(r, (distance + 1));
			float sin_beta = sin(alpha) * distance;
			float beta = asin(sin_beta);
			float theta = M_PI - alpha - beta;

			//rotation
			float z = cos(theta);
			float x = sin(theta)*cos(fi);
			float y = sin(theta)*sin(fi);
			float x1 = pRot[0] * x + pRot[1] * y + pRot[2] * z;
			float y1 = pRot[3] * x + pRot[4] * y + pRot[5] * z;
			float z1 = pRot[6] * x + pRot[7] * y + pRot[8] * z;
			theta = acos(z1);
			r = sqrt(x1*x1 + y1*y1);
			if (r < 0.00001) continue;
			fi = acos(x1 / r);
			if (y1 < 0)
			{
				fi = 2 * M_PI - fi;
			}

			float v_id = theta*panoH / M_PI;
			float u_id = (2 * M_PI - fi)*panoW / (2 * M_PI);

			int srcU0 = floor(u_id);
			int srcV0 = floor(v_id);

			int srcU1 = srcU0 + 1;
			int srcV1 = srcV0 + 1;

			srcU0 = (srcU0 > panoW - 1) ? (panoW - 1) : ((srcU0 < 0) ? (0) : srcU0);
			srcV0 = (srcV0 > panoH - 1) ? (panoH - 1) : ((srcV0 < 0) ? (0) : srcV0);
			srcU1 = (srcU1 > panoW - 1) ? (panoW - 1) : ((srcU1 < 0) ? (0) : srcU1);
			srcV1 = (srcV1 > panoH - 1) ? (panoH - 1) : ((srcV1 < 0) ? (0) : srcV1);

			float dx = u_id - srcU0;
			float dy = v_id - srcV0;

			float w0 = (1 - dx)*(1 - dy);
			float w1 = dx*(1 - dy);
			float w2 = (1 - dx)*dy;
			float w3 = dx*dy;

			int dstIdx = (i*colorSize.width + j)*channel;
			for (int chan = 0; chan < channel; chan++)
			{
				planeData[dstIdx + chan] = w0 * panoData[(srcV0*panoW + srcU0)*channel + chan]
					+ w1 * panoData[(srcV0*panoW + srcU1)* channel + chan]
					+ w2 * panoData[(srcV1*panoW + srcU0)*channel + chan]
					+ w3 * panoData[(srcV1*panoW + srcU1)*channel + chan];
			}
		}
	}
}


void main()
{
	std::string colorImgPath = "data/pano.jpg";
	cv::Mat img = cv::imread(colorImgPath, 1);

	cv::Mat R = cv::Mat::eye(3, 3, CV_32F);

	float anglex = 0;
	float angley = 0;
	float anglez = 120 * M_PI / 180;
	cv::Mat Rotx, Roty, Rotz;
	generateRotation(anglex, angley, anglez, Rotx, Roty, Rotz);
	R = Rotx * Roty* Rotz;

	float FOV = 120 * M_PI / 180;
	float distance = 1.0;
	cv::Mat planetColor, planetDisp;
	cv::Size outSize(3000, 3000);
	pano2Planet(img, planetColor, FOV, distance, R, outSize);

	cv::imwrite("data/planet.png", planetColor);
}